/*
 * Copyright (c) 2022-2023 Termony Co., Ltd. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of
 *    conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list
 *    of conditions and the following disclaimer in the documentation and/or other materials
 *    provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used
 *    to endorse or promote products derived from this software without specific prior written
 *    permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ASSEMBLY
#define ASSEMBLY 1
#endif

#include "los_arch_assembler.h"
#include "target_config.h"

FILE_BEGIN()

    /* Variables and functions. */
    EXPORT_LABEL(HalStartToRun)
    EXPORT_LABEL(HalTaskSwitch)
    EXPORT_LABEL(HalTaskSwitchFromISR)

    IMPORT_LABEL(g_losTask)
    IMPORT_LABEL(g_intCount)
    IMPORT_LABEL(g_taskYieldRequired)
    IMPORT_LABEL(g_taskHasFPUContext)
    IMPORT_LABEL(OsSchedTaskSwitch)
    IMPORT_LABEL(OsSignalTaskContextRestore)


/* 保存任务上下文. */
MACRO_DEF(SAVE_TASK_CONTEXT)
    /* 将PC和SPSR保存到系统模式栈中(即任务栈). */
    SRSDB   SP!, #SYS32_MODE
    /* 切换到系统模式. */
    CPS     #SYS32_MODE
    /* 将LR保存到任务栈中. */
    STMFD   SP!, {LR}
    /* 将SP保存到任务栈中. */
    MOV     LR, SP
    ADD     LR, LR, #(3 * 4)
    STMFD   SP!, {LR}
    /* 将R0 ~ R12保存到任务栈中. */
    STMFD   SP!, {R0-R12}

#if (LOSCFG_PLATFORM_FPU_ENABLE > 0)
    /* 保存浮点寄存器D0 ~ D31及FPSCR到任务栈中. */
    FMRX    R1, FPSCR
    PUSH    {R1}
    VPUSH   {D0-D15}
    VPUSH   {D16-D31}
#endif /* LOSCFG_PLATFORM_FPU_ENABLE */

    /* 修改任务栈指向SP. */
    LDR     R0, =g_losTask
    LDR     R1, [R0]
    STR     SP, [R1]
MACRO_END()


/* 恢复任务上下文. */
MACRO_DEF(RESTORE_TASK_CONTEXT)
    /* 修改SP指向任务栈. */
    LDR     R0, =g_losTask
    LDR     R1, [R0]
    LDR     SP, [R1]

#if (LOSCFG_PLATFORM_FPU_ENABLE > 0)
    /* 从任务栈中恢复浮点寄存器D0 ~ D31及FPSCR. */
    VPOP    {D16-D31}
    VPOP    {D0-D15}
    POP     {R0}
    VMSR    FPSCR, R0
#endif /* LOSCFG_PLATFORM_FPU_ENABLE */

    /* 从任务栈中恢复R0 ~ R12. */
    LDMFD   SP!, {R0-R12}
    /* 跳过任务栈中保存的SP. */
    ADD     SP, SP, #(1 * 4)
    /* 从任务栈中恢复LR. */
    LDMFD   SP!, {LR}
    /* 从任务栈中恢复PC和SPSR, 并返回到任务代码. */
    RFEIA   SP!
MACRO_END()


/* 恢复信号上下文. */
MACRO_DEF(SIGNAL_CONTEXT_RESTORE reg)
    /* 修改SP指向信号栈. */
    MOV     SP, \reg

#if (LOSCFG_PLATFORM_FPU_ENABLE > 0)
    /* 从任务栈中恢复浮点寄存器D0 ~ D31及FPSCR. */
    VPOP    {D16-D31}
    VPOP    {D0-D15}
    POP     {R0}
    VMSR    FPSCR, R0
#endif /* LOSCFG_PLATFORM_FPU_ENABLE */

    /* 从任务栈中恢复R0 ~ R12. */
    LDMFD   SP!, {R0-R12}
    /* 跳过任务栈中保存的SP. */
    ADD     SP, SP, #(1 * 4)
    /* 从任务栈中恢复LR. */
    LDMFD   SP!, {LR}
    /* 从任务栈中恢复PC和SPSR, 并返回到任务代码. */
    RFEIA   SP!
MACRO_END()


FUNC_DEF(HalStartToRun)
    /* 切换到系统模式. */
    CPS     #0x1f

    /* 让运行任务TCB指向新任务TCB, 用于切换任务. */
    LDR     R0, =g_losTask
    LDR     R1, [R0, #4]
    STR     R1, [R0]

    /* 恢复运行任务的上下文. */
    RESTORE_TASK_CONTEXT
    FUNC_END()


FUNC_DEF(HalTaskSwitch)
    /* 保存当前任务的上下文. */
    SAVE_TASK_CONTEXT

    /* 调用LiteOS-M的信号切换接口. */
    BLX     OsSignalTaskContextRestore
    CMP     R0, #0
    BEQ     TASK_SWITCH_CHECK
    /* 恢复信号上下文. */
    SIGNAL_CONTEXT_RESTORE R0

TASK_SWITCH_CHECK:
    /* 调用LiteOS-M的任务切换接口. */
    BLX     OsSchedTaskSwitch
    CMP     R0, #0
    BEQ     EXIT_NO_SWITCH

    /* 让运行任务TCB指向新任务TCB, 用于切换任务. */
    LDR     R0, =g_losTask
    LDR     R1, [R0, #4]
    STR     R1, [R0]

EXIT_NO_SWITCH:
    /* 恢复运行任务的上下文. */
    RESTORE_TASK_CONTEXT
    FUNC_END()


FUNC_DEF(HalTaskSwitchFromISR)
    /* 将寄存器R0, R1和LR保存到栈中. */
    PUSH    {R0, R1, LR}
    /* 将SPSR保存到栈中. */
    MRS     R0, SPSR
    PUSH    {R0}

    /* 如果中断嵌套计数不为0, 则不执行上下文切换. */
    LDR     R1, =g_intCount
    LDR     R0, [R1]
    CMP     R0, #0
    BNE     EXIT_WITHOUT_SWITCH

    /* 判断中断是否请求上下文切换. */
    LDR     R1, =g_taskYieldRequired
    LDR     R0, [R1]
    CMP     R0, #0
    BEQ     EXIT_WITHOUT_SWITCH

    /* 清除上下文切换请求标识. */
    MOV     R0, #0
    STR     R0, [R1]

    /* 从栈中恢复SPSR. */
    POP     {R0}
    MSR     SPSR_cxsf, R0
    /* 从栈中恢复寄存器R0, R1和LR. */
    POP     {R0, R1, LR}

    /* 切换任务上下文. */
    B       HalTaskSwitch

EXIT_WITHOUT_SWITCH:
    /* 从栈中恢复SPSR. */
    POP     {R0}
    MSR     SPSR_cxsf, R0
    /* 从栈中恢复寄存器R0, R1和LR. */
    POP     {R0, R1, LR}
    /* 跳转到中断返回地址(LR). */
    MOVS    PC, LR
    FUNC_END()

FILE_END()